
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_compass_calibrator.c
  * @author     baiyang
  * @date       2021-12-11
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_compass.h"

#include <rthw.h>
#include <rtthread.h>

#include <common/grapilot.h>
#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>
#include <mavproxy/mavproxy.h>
#include <parameter/param.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#ifndef COMPASS_CAL_GCS_SEND_TEXT
#define COMPASS_CAL_GCS_SEND_TEXT(severity, format, args...) mavproxy_send_statustext(severity, format, ##args)
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool _accept_calibration(uint8_t i);
static bool start_calibration_all(bool retry, bool autosave, float delay, bool autoreboot);
static bool _start_calibration(uint8_t i, bool retry, float delay);
static bool _start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot);
static void _cancel_calibration_mask(uint8_t mask);
static void _cancel_calibration(uint8_t i);
static void update_calibration_trampoline(void *parameter);
static bool _accept_calibration_mask(uint8_t mask);
static void cancel_calibration_all();
static uint8_t _get_cal_mask();
static bool _thread_create(const char *name, void (*entry)(void *parameter), uint32_t stack_size, uint8_t  priority);
/*----------------------------------variable----------------------------------*/
static sensor_compass* _compass;

static uitc_actuator_armed actuator_armed;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_compass_cal_init()
{
    _compass = sensor_compass_get_singleton();
}

void sensor_compass_cal_update()
{
    itc_copy_from_hub(ITC_ID(vehicle_actuator_armed), &actuator_armed);

    if (actuator_armed.armed) {
        return;
    }

    bool running = false;

    for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_compass->_calibrator[i] == NULL) {
            continue;
        }
        if (compasscal_failed(_compass->_calibrator[i])) {
            //AP_Notify::events.compass_cal_failed = 1;
        }

        if (compasscal_running(_compass->_calibrator[i])) {
            running = true;
        } else if (_compass->_cal_autosave && !_compass->_cal_saved[i] && (compasscal_get_state(_compass->_calibrator[i])).status == COMPASS_CAL_SUCCESS) {
            _accept_calibration((uint8_t)i);
        }
    }

    //AP_Notify::flags.compass_cal_running = running;

    if (sensor_compass_is_calibrating()) {
        _compass->_cal_has_run = true;
        return;
    } else if (_compass->_cal_has_run && _compass->_compass_cal_autoreboot) {
        rt_thread_mdelay(1000);
        rt_hw_cpu_reset();
    }
}

bool sensor_compass_is_calibrating()
{
    for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_compass->_calibrator[i] == NULL) {
            continue;
        }
        switch((compasscal_get_state(_compass->_calibrator[i])).status) {
            case COMPASS_CAL_NOT_STARTED:
            case COMPASS_CAL_SUCCESS:
            case COMPASS_CAL_FAILED:
            case COMPASS_CAL_BAD_ORIENTATION:
                break;
            default:
                return true;
        }
    }
    return false;
}

/*
  handle an incoming MAG_CAL command
 */
MAV_RESULT sensor_compass_handle_mag_cal_command(const mavlink_command_long_t* packet)
{
    MAV_RESULT result = MAV_RESULT_FAILED;

    switch (packet->command) {
    case MAV_CMD_DO_START_MAG_CAL: {
        result = MAV_RESULT_ACCEPTED;
        if (actuator_armed.armed) {
            COMPASS_CAL_GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Disarm to allow compass calibration");
            result = MAV_RESULT_FAILED;
            break;
        }
        if (packet->param1 < 0 || packet->param1 > 255) {
            result = MAV_RESULT_FAILED;
            break;
        }

        uint8_t mag_mask = packet->param1;
        bool retry = !math_flt_zero(packet->param2);
        bool autosave = !math_flt_zero(packet->param3);
        float delay = packet->param4;
        bool autoreboot = !math_flt_zero(packet->param5);

        if (mag_mask == 0) { // 0 means all
            sensor_compass_reset_compass_id();
            if (!start_calibration_all(retry, autosave, delay, autoreboot)) {
                result = MAV_RESULT_FAILED;
            }
        } else {
            if (!_start_calibration_mask(mag_mask, retry, autosave, delay, autoreboot)) {
                result = MAV_RESULT_FAILED;
            }
        }

        break;
    }

    case MAV_CMD_DO_ACCEPT_MAG_CAL: {
        result = MAV_RESULT_ACCEPTED;
        if (packet->param1 < 0 || packet->param1 > 255) {
            result = MAV_RESULT_FAILED;
            break;
        }

        uint8_t mag_mask = packet->param1;

        if (mag_mask == 0) { // 0 means all
            mag_mask = 0xFF;
        }

        if (!_accept_calibration_mask(mag_mask)) {
            result = MAV_RESULT_FAILED;
        }
        break;
    }

    case MAV_CMD_DO_CANCEL_MAG_CAL: {
        result = MAV_RESULT_ACCEPTED;
        if (packet->param1 < 0 || packet->param1 > 255) {
            result = MAV_RESULT_FAILED;
            break;
        }
        
        uint8_t mag_mask = packet->param1;
        
        if (mag_mask == 0) { // 0 means all
            cancel_calibration_all();
            break;
        }
        
        _cancel_calibration_mask(mag_mask);
        break;
    }
    }
    
    return result;
}

bool sensor_compass_send_mag_cal_progress(mavlink_message_t *msg_t)
{
    const mavlink_channel_t chan = MAVLINK_COMM_0;

    for (uint8_t i = 0; i < COMPASS_MAX_INSTANCES; i++) {
        const CompassPriority compass_id = (_compass->next_cal_progress_idx[chan] + 1) % COMPASS_MAX_INSTANCES;
        
        CompassCalibrator* calibrator = _compass->_calibrator[compass_id];
        if (calibrator == NULL) {
            _compass->next_cal_progress_idx[chan] = compass_id;
            continue;
        }

        const struct CompassCalState cal_state = compasscal_get_state(calibrator);
        mavlink_mag_cal_progress_t mag_cal_progress = {0};

        mavlink_system_t system_id = mavproxy_get_system();

        if (cal_state.status == COMPASS_CAL_WAITING_TO_START  ||
            cal_state.status == COMPASS_CAL_RUNNING_STEP_ONE ||
            cal_state.status == COMPASS_CAL_RUNNING_STEP_TWO) {
            // ensure we don't try to send with no space available

            _compass->next_cal_progress_idx[chan] = compass_id;
            
            mag_cal_progress.compass_id = compass_id;
            mag_cal_progress.cal_mask   = _get_cal_mask();
            mag_cal_progress.cal_status = (uint8_t)cal_state.status;
            mag_cal_progress.attempt    = cal_state.attempt;
            mag_cal_progress.completion_pct  = cal_state.completion_pct;
            mag_cal_progress.direction_x = 0.0f;
            mag_cal_progress.direction_y = 0.0f;
            mag_cal_progress.direction_z = 0.0f;
            mav_array_memcpy(mag_cal_progress.completion_mask, cal_state.completion_mask, sizeof(uint8_t)*10);

            mavlink_msg_mag_cal_progress_encode(system_id.sysid, system_id.compid, msg_t, &mag_cal_progress);
        } else {
            _compass->next_cal_progress_idx[chan] = compass_id;
        }
    }

    return true;
}

bool sensor_compass_send_mag_cal_report(mavlink_message_t *msg_t)
{
    const mavlink_channel_t chan = MAVLINK_COMM_0;

    for (uint8_t i = 0; i < COMPASS_MAX_INSTANCES; i++) {
        const CompassPriority compass_id = (_compass->next_cal_report_idx[chan] + 1) % COMPASS_MAX_INSTANCES;

        if (_compass->_calibrator[compass_id] == NULL) {
            _compass->next_cal_report_idx[chan] = compass_id;
            continue;
        }

        const struct CompassCalReport cal_report = compasscal_get_report(_compass->_calibrator[compass_id]);
        mavlink_mag_cal_report_t mag_cal_report = {0};

        mavlink_system_t system_id = mavproxy_get_system();

        if (cal_report.status == COMPASS_CAL_SUCCESS ||
            cal_report.status == COMPASS_CAL_FAILED ||
            cal_report.status == COMPASS_CAL_BAD_ORIENTATION) {

            _compass->next_cal_report_idx[chan] = compass_id;

            mag_cal_report.compass_id = compass_id;
            mag_cal_report.cal_mask   = _get_cal_mask();
            mag_cal_report.cal_status = (uint8_t)cal_report.status;
            mag_cal_report.autosaved  = _compass->_cal_saved[compass_id];
            mag_cal_report.fitness    = cal_report.fitness;
            mag_cal_report.ofs_x      = cal_report.ofs.x;
            mag_cal_report.ofs_y      = cal_report.ofs.y;
            mag_cal_report.ofs_z      = cal_report.ofs.z;
            mag_cal_report.diag_x     = cal_report.diag.x;
            mag_cal_report.diag_y     = cal_report.diag.y;
            mag_cal_report.diag_z     = cal_report.diag.z;
            mag_cal_report.offdiag_x  = cal_report.offdiag.x;
            mag_cal_report.offdiag_y  = cal_report.offdiag.y;
            mag_cal_report.offdiag_z  = cal_report.offdiag.z;
            mag_cal_report.orientation_confidence = cal_report.orientation_confidence;
            mag_cal_report.old_orientation = cal_report.original_orientation;
            mag_cal_report.new_orientation = cal_report.orientation;
            mag_cal_report.scale_factor    = cal_report.scale_factor;

            mavlink_msg_mag_cal_report_encode(system_id.sysid, system_id.compid, msg_t, &mag_cal_report);
        } else {
            _compass->next_cal_report_idx[chan] = compass_id;
        }
    }

    return true;
}

static bool _accept_calibration(uint8_t i)
{
    CompassPriority prio = (CompassPriority)i;

    CompassCalibrator* cal = _compass->_calibrator[prio];
    if (cal == NULL) {
        return false;
    }
    const struct CompassCalReport cal_report = compasscal_get_report(cal);

    if (_compass->_cal_saved[prio] || cal_report.status == COMPASS_CAL_NOT_STARTED) {
        return true;
    } else if (cal_report.status == COMPASS_CAL_SUCCESS) {
        _compass->_cal_saved[prio] = true;

        sensor_compass_set_and_save_offsets(i, &cal_report.ofs);
        sensor_compass_set_and_save_diagonals(i,&cal_report.diag);
        sensor_compass_set_and_save_offdiagonals(i,&cal_report.offdiag);
        sensor_compass_set_and_save_scale_factor(i,cal_report.scale_factor);

        if (cal_report.check_orientation && sensor_compass_get_state(prio)->external && _compass->_rotate_auto >= 2) {
            sensor_compass_set_and_save_orientation(i, cal_report.orientation);
        }

        if (!sensor_compass_is_calibrating()) {
            //AP_Notify::events.compass_cal_saved = 1;
        }
        return true;
    } else {
        return false;
    }
}

static bool start_calibration_all(bool retry, bool autosave, float delay, bool autoreboot)
{
    _compass->_cal_autosave = autosave;
    _compass->_compass_cal_autoreboot = autoreboot;

    bool at_least_one_started = false;
    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
        // ignore any compasses that fail to start calibrating
        // start all should only calibrate compasses that are being used
        if (_start_calibration(i,retry,delay)) {
            at_least_one_started = true;
        }
    }
    return at_least_one_started;
}

static bool _start_calibration(uint8_t i, bool retry, float delay)
{
    if (!sensor_compass_healthy2(i)) {
        return false;
    }
    if (!sensor_compass_use_for_yaw2(i)) {
        return false;
    }
    CompassPriority prio = (CompassPriority)i;

#if COMPASS_MAX_INSTANCES > 1
    if (_compass->_priority_did_list[prio] != _compass->_priority_did_stored_list[prio]) {
        COMPASS_CAL_GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Compass cal requires reboot after priority change");
        return false;
    }
#endif
    
    if (_compass->_calibrator[prio] == NULL) {
        _compass->_calibrator[prio] = (CompassCalibrator *)rt_malloc(sizeof(CompassCalibrator));
        compasscal_ctor(_compass->_calibrator[prio], prio);

        if (_compass->_calibrator[prio] == NULL) {
            COMPASS_CAL_GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Compass cal object not initialised");
            return false;
        }
    }

    if (_compass->_options & CAL_REQUIRE_GPS) {
        uitc_sensor_gps gps_pos_t = {0};
        int res = itc_copy_from_hub(ITC_ID(sensor_gps), &gps_pos_t);
        if (res != 0 || gps_pos_t.fix_type < GPS_OK_FIX_2D) {
            COMPASS_CAL_GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Compass cal requires GPS lock");
            return false;
        }
    }
    if (!sensor_compass_is_calibrating()) {
        //AP_Notify::events.initiated_compass_cal = 1;
    }

    if (_compass->_rotate_auto) {
        enum RotationEnum r = (sensor_compass_get_state(prio))->external?(enum RotationEnum)(sensor_compass_get_state(prio))->orientation:ROTATION_NONE;
        if (r != ROTATION_CUSTOM) {
            compasscal_set_orientation(_compass->_calibrator[prio], 
                                       r,
                                       (sensor_compass_get_state(prio))->external, 
                                       _compass->_rotate_auto>=2, 
                                       _compass->_rotate_auto>=3);
        }
    }
    _compass->_cal_saved[prio] = false;
    if (i == 0 && (sensor_compass_get_state(prio))->external != 0) {
        compasscal_start(_compass->_calibrator[prio], retry, delay, (uint16_t)_compass->_offset_max, i, _compass->_calibration_threshold);
    } else {
        // internal compasses or secondary compasses get twice the
        // threshold. This is because internal compasses tend to be a
        // lot noisier
        compasscal_start(_compass->_calibrator[prio], retry, delay, (uint16_t)_compass->_offset_max, i, _compass->_calibration_threshold*2);
    }
    if (!_compass->_cal_thread_started) {
        _compass->_cal_requires_reboot = true;
        if (!_thread_create("magcal", update_calibration_trampoline , 1024*8, PRIORITY_IO)) {
            COMPASS_CAL_GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "CompassCalibrator: Cannot start compass thread.");
            return false;
        }
        _compass->_cal_thread_started = true;
    }

    // disable compass learning both for calibration and after completion
    param_set_and_save(PARAM_ID(COMPASS, COMPASS_LEARN), 0);

    return true;
}

static bool _start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot)
{
    _compass->_cal_autosave = autosave;
    _compass->_compass_cal_autoreboot = autoreboot;

    bool at_least_one_started = false;
    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
        if ((1<<i) & mask) {
            if (!_start_calibration(i,retry,delay)) {
                _cancel_calibration_mask(mask);
                return false;
            }
            at_least_one_started = true;
        }
    }
    return at_least_one_started;
}

static void _cancel_calibration_mask(uint8_t mask)
{
    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
        if ((1<<i) & mask) {
            _cancel_calibration(i);
        }
    }
}

static void _cancel_calibration(uint8_t i)
{
    //AP_Notify::events.initiated_compass_cal = 0;
    CompassPriority prio = (CompassPriority)i;
    if (_compass->_calibrator[prio] == NULL) {
        return;
    }
    if (compasscal_running(_compass->_calibrator[prio]) || (compasscal_get_state(_compass->_calibrator[prio])).status == COMPASS_CAL_WAITING_TO_START) {
        //AP_Notify::events.compass_cal_canceled = 1;
    }
    _compass->_cal_saved[prio] = false;
    compasscal_stop(_compass->_calibrator[prio]);
}

static void update_calibration_trampoline(void *parameter)
{
    while(true) {
        for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
            if (_compass->_calibrator[i] == NULL) {
                continue;
            }
            compasscal_update(_compass->_calibrator[i]);
        }
        rt_thread_mdelay(1);
    }
}

static bool _accept_calibration_mask(uint8_t mask)
{
    bool success = true;
    for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_compass->_calibrator[i] == NULL) {
            continue;
        }
        if ((1<<(uint8_t)(i)) & mask) {
            if (!_accept_calibration((uint8_t)i)) {
                success = false;
            }
            compasscal_stop(_compass->_calibrator[i]);
        }
    }

    return success;
}

static void cancel_calibration_all()
{
    _cancel_calibration_mask(0xFF);
}

static uint8_t _get_cal_mask()
{
    uint8_t cal_mask = 0;
    for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_compass->_calibrator[i] == NULL) {
            continue;
        }
        if (compasscal_get_state(_compass->_calibrator[i]).status != COMPASS_CAL_NOT_STARTED) {
            cal_mask |= 1 << i;
        }
    }
    return cal_mask;
}

static bool _thread_create(const char *name, void (*entry)(void *parameter), uint32_t stack_size, uint8_t  priority)
{
    rt_err_t res = RT_ERROR;

    rt_thread_t thr = rt_thread_create(name, entry, NULL, stack_size, priority, 2);

    if (thr != NULL) {
        res = rt_thread_startup(thr);
    }

    return res == RT_EOK;
}

/*------------------------------------test------------------------------------*/


